#include <unionsys_core.hpp>

RegisterMSN *map_location_core = new RegisterMSN();


const float deg2rad = 3.1415926535798/180.0;
const float rad2deg = 180.0/3.1415926535798;




geometry_msgs::Vector3 toEulerAngle(geometry_msgs::Quaternion quat);
geometry_msgs::Point rotation_hehe(geometry_msgs::Vector3 angle);
geometry_msgs::Quaternion quatMultiply(geometry_msgs::Quaternion q1, geometry_msgs::Quaternion q2);
Eigen::Matrix3d Quaternion2RotationMatrix(const double x,const double y,const double z,const double w);
geometry_msgs::Vector3 R_multi_P(Eigen::Matrix3d R, double x, double y, double z);
Eigen::Matrix3d R_multi_R(Eigen::Matrix3d R1, Eigen::Matrix3d R2);


ros::Subscriber tagdect_sub;
ros::Subscriber odometrysub;
ros::Publisher map_location_pub, tag_flag_pub, map_odom_pub;

geometry_msgs::Point current_point;
geometry_msgs::Quaternion current_angle;
geometry_msgs::Vector3 curr_angle;
float current_yaw_angle;

double tag_x_0, tag_y_0, tag_z_0, tag_yaw_0;
double tag_x_1, tag_y_1, tag_z_1, tag_yaw_1;
double tag_x_2, tag_y_2, tag_z_2, tag_yaw_2;
double tag_x_3, tag_y_3, tag_z_3, tag_yaw_3;
double tag_x_4, tag_y_4, tag_z_4, tag_yaw_4;
double tag_x_5, tag_y_5, tag_z_5, tag_yaw_5;
double tag_x_6, tag_y_6, tag_z_6, tag_yaw_6;
double tag_x_7, tag_y_7, tag_z_7, tag_yaw_7;
double tag_x_8, tag_y_8, tag_z_8, tag_yaw_8;
double tag_array[36];

Eigen::Matrix3d R_plane_cam, R_tag_world;
geometry_msgs::Quaternion Q_plane_cam, Q_tag_world;
geometry_msgs::Vector3 P_plane_cam, P_tag_world;
bool tag_detected_flag = false;
int seq_cnt = 0;
geometry_msgs::Vector3 P_offset;
geometry_msgs::Quaternion Q_offset;
